///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Contains code for 4x4 matrices.
 *	\file		IceMatrix4x4.cpp
 *	\author		Pierre Terdiman
 *	\date		April, 4, 2000
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	4x4 matrix.
 *	DirectX-compliant, ie row-column order, ie m[Row][Col].
 *	Same as:
 *	m11  m12  m13  m14	first row.
 *	m21  m22  m23  m24	second row.
 *	m31  m32  m33  m34	third row.
 *	m41  m42  m43  m44	fourth row.
 *	Translation is (m41, m42, m43), (m14, m24, m34, m44) = (0, 0, 0, 1).
 *	Stored in memory as m11 m12 m13 m14 m21...
 *
 *	Multiplication rules:
 *
 *	[x'y'z'1] = [xyz1][M]
 *
 *	x' = x*m11 + y*m21 + z*m31 + m41
 *	y' = x*m12 + y*m22 + z*m32 + m42
 *	z' = x*m13 + y*m23 + z*m33 + m43
 *	1' =     0 +     0 +     0 + m44
 *
 *	\class		Matrix4x4
 *	\author		Pierre Terdiman
 *	\version	1.0
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Constructor rotate about origin by point
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include "Math\LaborMathInclude.h"

NAMESPACE_LABOR3D_BEGIN

const CLMatrix4x4 CLMatrix4x4::ZERO(
							0, 0, 0, 0,
							0, 0, 0, 0,
							0, 0, 0, 0,
							0, 0, 0, 0 );

const CLMatrix4x4 CLMatrix4x4::IDENTITY(
								1, 0, 0, 0,
								0, 1, 0, 0,
								0, 0, 1, 0,
								0, 0, 0, 1 );

const CLMatrix4x4 CLMatrix4x4::CLIPSPACE2DTOIMAGESPACE(
	0.5,    0,  0, 0.5, 
	0, -0.5,  0, 0.5, 
	0,    0,  1,   0,
	0,    0,  0,   1);



CLMatrix4x4::CLMatrix4x4( CLPoint point )
{
	enum {
		AXIS_X   = (1<<0),
		AXIS_Y   = (1<<1),
		AXIS_Z   = (1<<2)
	};

	unsigned long axis = 0;
	if (point[0] != 0.0f) axis |= AXIS_X;
	if (point[1] != 0.0f) axis |= AXIS_Y;
	if (point[2] != 0.0f) axis |= AXIS_Z;

	switch (axis)
	{
	case 0:
		identity();
		break;

	case AXIS_X:
		{
			float cx,sx;
			sx = sinf( point[0] );
			cx = cosf( point[0] );

			m[0][0] = 1.0f;
			m[0][1] = 0.0f;
			m[0][2] = 0.0f;
			m[0][3] = 0.0f;

			m[1][0] = 0.0f;
			m[1][1] = cx;
			m[1][2] = sx;
			m[1][3] = 0.0f;

			m[2][0] = 0.0f;
			m[2][1] = -sx;
			m[2][2]= cx;
			m[2][3]= 0.0f;

			m[3][0]= 0.0f;
			m[3][1]= 0.0f;
			m[3][2]= 0.0f;
			m[3][3]= 1.0f;
			break;
		}

	case AXIS_Y:
		{
			float cy,sy;
			sy = sinf( point[1] );
			cy = cosf( point[1] );

			m[0][0] = cy;
			m[0][1] = 0.0f;
			m[0][2] = -sy;
			m[0][3] = 0.0f;

			m[1][0] = 0.0f;
			m[1][1] = 1.0f;
			m[1][2] = 0.0f;
			m[1][3] = 0.0f;

			m[2][0] = sy;
			m[2][1] = 0.0f;
			m[2][2]=  cy;
			m[2][3]=  0.0f;

			m[3][0]=  0.0f;
			m[3][1]=  0.0f;
			m[3][2]=  0.0f;
			m[3][3]=  1.0f;
			break;
		}

	case AXIS_Z:
		{
			// the matrix looks like this:
			//  r1 - (r4 * sin(x))     r2 + (r3 * sin(x))   -cos(x) * sin(y)
			//  -cos(x) * sin(z)       cos(x) * cos(z)      sin(x)
			//  r3 + (r2 * sin(x))     r4 - (r1 * sin(x))   cos(x) * cos(y)
			//
			// where:
			//  r1 = cos(y) * cos(z)
			//  r2 = cos(y) * sin(z)
			//  r3 = sin(y) * cos(z)
			//  r4 = sin(y) * sin(z)
			float cz,sz;
			//mSinCos( point[2], sz, cz );
			sz = sinf( point[2] );
			cz = cosf( point[2] );
			float r1 = cz;
			float r2 = sz;
			float r3 = 0.0f;
			float r4 = 0.0f;

			m[0][0] = cz;
			m[0][1] = sz;
			m[0][2] = 0.0f;
			m[0][3] = 0.0f;

			m[1][0] = -sz;
			m[1][1] = cz;
			m[1][2] = 0.0f;
			m[1][3] = 0.0f;

			m[2][0] = 0.0f;
			m[2][1] = 0.0f;
			m[2][2] = 1.0f;
			m[2][3] = 0.0f;

			m[3][0] = 0.0f;
			m[3][1] = 0.0f;
			m[3][2] = 0.0f;
			m[3][3] = 1.0f;
			break;
		}

	default:
		// the matrix looks like this:
		//  r1 - (r4 * sin(x))     r2 + (r3 * sin(x))   -cos(x) * sin(y)
		//  -cos(x) * sin(z)       cos(x) * cos(z)      sin(x)
		//  r3 + (r2 * sin(x))     r4 - (r1 * sin(x))   cos(x) * cos(y)
		//
		// where:
		//  r1 = cos(y) * cos(z)
		//  r2 = cos(y) * sin(z)
		//  r3 = sin(y) * cos(z)
		//  r4 = sin(y) * sin(z)
		float cx,sx;
//		mSinCos( point[0], sx, cx );
		sx = sinf( point[0] );
		cx = cosf( point[0] );
		float cy,sy;
//		mSinCos( point[1], sy, cy );
		sy = sinf( point[1] );
		cy = cosf( point[1] );
		float cz,sz;
//		mSinCos( point[2], sz, cz );
		sz = sinf( point[2] );
		cz = cosf( point[2] );

		float r1 = cy * cz;
		float r2 = cy * sz;
		float r3 = sy * cz;
		float r4 = sy * sz;

		m[0][0] =r1 - (r4 * sx);
		m[0][1] =r2 + (r3 * sx);
		m[0][2] =-cx * sy;
		m[0][3] =0.0f;

		m[1][0] =-cx * sz;
		m[1][1] =cx * cz;
		m[1][2] =sx;
		m[1][3] =0.0f;

		m[2][0] =r3 + (r2 * sx);
		m[2][1] =r4 - (r1 * sx);
		m[2][2]= cx * cy;
		m[2][3]= 0.0f;

		m[3][0]= 0.0f;
		m[3][1]= 0.0f;
		m[3][2]= 0.0f;
		m[3][3]= 1.0f;
		break;
	}
}

const CLMatrix4x4& CLMatrix4x4::getIndentity()
{
	static CLMatrix4x4 matrixIndentity( CLPoint( 0.f, 0.f, 0.f ) );
	return matrixIndentity;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Inverts a PR matrix. (which only contains a rotation and a translation)
 *	This is faster and less subject to FPU errors than the generic inversion code.
 *
 *	\relates	Matrix4x4
 *	\fn			InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src)
 *	\param		dest	[out] destination matrix
 *	\param		src		[in] source matrix
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 void invertPRMatrix(CLMatrix4x4& dest, const CLMatrix4x4& src)
{
	dest.m[0][0] = src.m[0][0];
	dest.m[1][0] = src.m[0][1];
	dest.m[2][0] = src.m[0][2];
	dest.m[3][0] = -(src.m[3][0]*src.m[0][0] + src.m[3][1]*src.m[0][1] + src.m[3][2]*src.m[0][2]);

	dest.m[0][1] = src.m[1][0];
	dest.m[1][1] = src.m[1][1];
	dest.m[2][1] = src.m[1][2];
	dest.m[3][1] = -(src.m[3][0]*src.m[1][0] + src.m[3][1]*src.m[1][1] + src.m[3][2]*src.m[1][2]);

	dest.m[0][2] = src.m[2][0];
	dest.m[1][2] = src.m[2][1];
	dest.m[2][2] = src.m[2][2];
	dest.m[3][2] = -(src.m[3][0]*src.m[2][0] + src.m[3][1]*src.m[2][1] + src.m[3][2]*src.m[2][2]);

	dest.m[0][3] = 0.0f;
	dest.m[1][3] = 0.0f;
	dest.m[2][3] = 0.0f;
	dest.m[3][3] = 1.0f;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Compute the cofactor of the Matrix at a specified location
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float CLMatrix4x4::coFactor(unsigned int row, unsigned int col) const
{
	return	 (( m[(row+1)&3][(col+1)&3]*m[(row+2)&3][(col+2)&3]*m[(row+3)&3][(col+3)&3] +
				m[(row+1)&3][(col+2)&3]*m[(row+2)&3][(col+3)&3]*m[(row+3)&3][(col+1)&3] +
				m[(row+1)&3][(col+3)&3]*m[(row+2)&3][(col+1)&3]*m[(row+3)&3][(col+2)&3])
			-  (m[(row+3)&3][(col+1)&3]*m[(row+2)&3][(col+2)&3]*m[(row+1)&3][(col+3)&3] +
				m[(row+3)&3][(col+2)&3]*m[(row+2)&3][(col+3)&3]*m[(row+1)&3][(col+1)&3] +
				m[(row+3)&3][(col+3)&3]*m[(row+2)&3][(col+1)&3]*m[(row+1)&3][(col+2)&3])) * ((row + col) & 1 ? -1.0f : +1.0f);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Compute the determinant of the Matrix
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float CLMatrix4x4::determinant() const
{
	return	m[0][0] * coFactor(0, 0) +
			m[0][1] * coFactor(0, 1) +
			m[0][2] * coFactor(0, 2) +
			m[0][3] * coFactor(0, 3);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Compute the inverse of the matrix
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
CLMatrix4x4& CLMatrix4x4::invert()
{
	float Det = determinant();
	CLMatrix4x4 Temp;

	if(fabsf(Det) < MATRIX4X4_EPSILON)
		return	*this;		// The matrix is not invertible! Singular case!

	float IDet = 1.0f / Det;

	Temp.m[0][0] = coFactor(0,0) * IDet;
	Temp.m[1][0] = coFactor(0,1) * IDet;
	Temp.m[2][0] = coFactor(0,2) * IDet;
	Temp.m[3][0] = coFactor(0,3) * IDet;
	Temp.m[0][1] = coFactor(1,0) * IDet;
	Temp.m[1][1] = coFactor(1,1) * IDet;
	Temp.m[2][1] = coFactor(1,2) * IDet;
	Temp.m[3][1] = coFactor(1,3) * IDet;
	Temp.m[0][2] = coFactor(2,0) * IDet;
	Temp.m[1][2] = coFactor(2,1) * IDet;
	Temp.m[2][2] = coFactor(2,2) * IDet;
	Temp.m[3][2] = coFactor(2,3) * IDet;
	Temp.m[0][3] = coFactor(3,0) * IDet;
	Temp.m[1][3] = coFactor(3,1) * IDet;
	Temp.m[2][3] = coFactor(3,2) * IDet;
	Temp.m[3][3] = coFactor(3,3) * IDet;

	*this = Temp;

	return	*this;
}


CLMatrix4x4&			CLMatrix4x4::affineInvert()
{
	// Matrix class checks to make sure this is an affine transform before calling
	//  this function, so we can proceed assuming it is...
	float temp[16];
	memcpy( temp, m, 16 * sizeof(float) );

	// Transpose rotation
	m[0][1] = temp[4];
	m[1][0] = temp[1];
	m[0][2] = temp[8];
	m[2][1] = temp[2];
	m[1][2] = temp[9];
	m[2][2] = temp[6];

	m[0][3]  = -(temp[0]*temp[3] + temp[4]*temp[7] + temp[8]*temp[11]);
	m[1][3]  = -(temp[1]*temp[3] + temp[5]*temp[7] + temp[9]*temp[11]);
	m[2][3] = -(temp[2]*temp[3] + temp[6]*temp[7] + temp[10]*temp[11]);

	return *this;
}

CLMatrix4x4&			CLMatrix4x4::rot(float angle, const CLPoint& axis )
{
	/* This function performs an axis/angle rotation. (x,y,z) is any 
	vector on the axis. For greater speed, always use a unit vector 
	(length = 1). In this version, we will assume an arbitrary 
	length and normalize. */
	// normalize
	//float length = sqrtf(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); 
	float length = axis.getLength(); 
	// too close to 0, can't make a normalized vector
	if (length < 0.000001f)
		return *this;

	float x = axis.x / length;
	float y = axis.y / length;
	float z = axis.z / length;

	// do the trig
	float c = cosf(-angle);
	float s = sinf(-angle);
	float t = 1.0f - c;   

	// build the rotation matrix
	m[0][0] = t*x*x + c;
	m[0][1] = t*x*y - s*z;
	m[0][2] = t*x*z + s*y;
	m[0][3] = 0;

	m[1][0] = t*x*y + s*z;
	m[1][1] = t*y*y + c;
	m[1][2] = t*y*z - s*x;
	m[1][3] = 0;

	m[2][0] = t*x*z - s*y;
	m[2][1] = t*y*z + s*x;
	m[2][2] = t*z*z + c;
	m[2][3] = 0;

	m[3][0] = m[3][1] = m[3][2] = 0.0f;
	m[3][3] = 1.0f;

	return *this;
}

CLMatrix4x4& CLMatrix4x4::buildReflectionMatrix( CLPlane& p )
{
	set(
		-2 * p.n.x * p.n.x + 1,   -2 * p.n.x * p.n.y,       -2 * p.n.x * p.n.z,       -2 * p.n.x * p.d, 
		-2 * p.n.y * p.n.x,       -2 * p.n.y * p.n.y + 1,   -2 * p.n.y * p.n.z,       -2 * p.n.y * p.d, 
		-2 * p.n.z * p.n.x,       -2 * p.n.z * p.n.y,       -2 * p.n.z * p.n.z + 1,   -2 * p.n.z * p.d, 
		0,                        0,                        0,                        1);

	return *this;

}

CLMatrix4x4& CLMatrix4x4::buildReflectionMatrix( const CLPlane& p )
{
	set(
		-2 * p.n.x * p.n.x + 1,   -2 * p.n.x * p.n.y,       -2 * p.n.x * p.n.z,       -2 * p.n.x * p.d, 
		-2 * p.n.y * p.n.x,       -2 * p.n.y * p.n.y + 1,   -2 * p.n.y * p.n.z,       -2 * p.n.y * p.d, 
		-2 * p.n.z * p.n.x,       -2 * p.n.z * p.n.y,       -2 * p.n.z * p.n.z + 1,   -2 * p.n.z * p.d, 
		0,                        0,                        0,                        1);

	return *this;

}

void CLMatrix4x4::noScale()
{
	CLPoint vRow[3];
	vRow[0] = this->getRow(0);
	vRow[1] = this->getRow(1);
	vRow[2] = this->getRow(2);
	float squrt[3];
	squrt[0] = vRow[0].magnitude();
	squrt[1] = vRow[1].magnitude();
	squrt[2] = vRow[2].magnitude();
	float fScaling = sqrtf(m[0][0]*m[0][0] + m[1][1]*m[1][1] + m[2][2]*m[2][2]);
	for(int iRow=0; iRow<3; iRow++)
	{
		for(int jClo=0; jClo<3; jClo++)
		{
			m[iRow][jClo] /= squrt[iRow];
		}
	}
}

NAMESPACE_LABOR3D_END